//
// Created by Administrator on 2022/7/14.
//
//#include "string"
//
//using namespace std;

#ifndef CORE_STM32F4_FW_USER_DEFINE_H
#define CORE_STM32F4_FW_USER_DEFINE_H

#include "stdbool.h"
#include "cmsis_os2.h"

//#define ROBOT_BOOT

#define SOFT_VER_A 0
#define SOFT_VER_B 1
#define SOFT_VER_C 10

//#define DUMMY_T
//#define AR3_T
//#define SITO_T
//#define BATTLE_CHARGE_THERE_AXIS_T
//#define MR6_T
//#define IMU6_T
#define IRC_T


#define HARD_VER_SERIES_NUM 0
#define HARD_VER_ROBOT_NUM 1

#define CHARGE_PANEL_SUM 16

#define TRANMIT_EMPTY_BAT_ROBOT_SERIES_NUM 0
#define TRANMIT_EMPTY_BAT_ROBOT_ROBOT_NUM 2

#define TRANMIT_FULL_BAT_ROBOT_SERIES_NUM 0
#define TRANMIT_FULL_BAT_ROBOT_ROBOT_NUM 3

#ifdef __cplusplus
extern "C" {
#endif


typedef struct{
    unsigned short  c;      //[0-65536]
    unsigned short  r;
    unsigned short  g;
    unsigned short  b;
}COLOR_RGBC;//RGBC

#if defined(IMU6_T) || defined(IRC_T)
typedef struct{
    unsigned short h;       //[0,360]
    unsigned char  s;       //[0,100]
    unsigned char  l;       //[0,100]
}COLOR_HSL;//HSL

typedef struct{
    unsigned char  r;     //[0-255]
    unsigned char  g;
    unsigned char  b;
    unsigned short Lux;
}Sepan_RGBC;//RGBC

#define MotorSpeed 10


typedef enum{
    Open = 0,
    Close,
    Up,
    Down
} Driver_Status;


typedef struct {
    uint8_t recflag;
    uint16_t reclen;
    uint8_t buff[1024];
}DataRec;

    extern uint8_t  ReadPosFLG;
typedef struct{
    bool Find_Home;
    uint8_t Elector1;
    uint8_t Elector2;
    uint8_t Color_Flag;
    float  data;
    uint32_t Start_time ;
    uint32_t Signal_time1;
    uint32_t Signal_time2;
    COLOR_RGBC rgb;
    COLOR_HSL hsl;
}IMU6_SIGNAL;
extern IMU6_SIGNAL imu6_data[5] ;
extern  float ZeroData[5] ;
extern uint8_t  IMU6_Work;
#endif

typedef uint32_t u32;
typedef uint16_t u16;
typedef uint8_t u8;

enum HandConMode
{
    HAND_CON_BY_ANGLE = 1,
    HAND_CON_BY_POWER,
    HAND_CON_BY_CURRENT,
    HAND_CON_BY_VELOCITY,
};

#define USER_DATA_SOCTOR_SIZE 16 * 1024

#define FLASH_BOOTLOADER0_ADDR				0x08000000  	//0x08000000 - 0x08007FFF (Sector0 1，共32KB)，为Bootloader0

#define FLASH_USER_DATA_ADDR				0x08008000  	//0x08008000 - 0x0800BFFF (Sector2, 16 KB)
#define FLASH_USER_DATA_BACKUP_ADDR			0x0800C000  	//0x0800C000 - 0x0800FFFF (Sector3, 16 KB)

#define FLASH_BOOTLOADER1_ADDR				0x08010000  	//0x08010000 - 0x0801FFFF (Sector4，共64KB)，为用户数据

#define FLASH_APP0_ADDR				0x08020000  	//0x08020000 - 0x0807FFFF (Sector5 6 7，共384KB)，为APP0
#define FLASH_APP1_ADDR				0x08080000   	//0x08080000 - 0x080DFFFF  (Sector8 9 10，共384KB)，为APP1


#define DANCE_FLASH_ADDR 0x080E0000
#define DANCE_HAND_SIZE 128
#define PER_STEP_SIZE 48

extern bool InitPosFalg[7],AdjustMagFlag[7],OverLoadFlag[7];
extern float MpuAngle[7];
#define FIND_INIT_POS_SPEED ((float)1/20)
#define MPU_ANGLE_MIN -100
#define MPU_ANGLE_MAX 100

typedef enum HARD_VER{
    HARD_VER_ERR = 0,
    DUMMY,
    AR3,
    SITO,
    BATTLE_CHARGE_THERE_AXIS,
    MR6,
    IMU6,
    IRC,

}Enum_16bit;


enum BAT_CHARGE_STA{
    ERR_FULL_STA = -3,
    ERR_EMPTY_STA = -2,
    NO_WAIT_STA_UPDATA = -1,
    NO_BAT = 1,
    BAT_CHARGING = 2,
    BAT_FULL = 3,
    BAT_ERR = 0xE0,

    BAT_ERR_GETTED = 0xF0,
};

enum ROBOT_LOG_TYPE{
    UNKNOWN_ROBOT_LOG_YYPE = 0,
    POWER_ON_LOG = 1,
    TYPE_C_COMMAND_LOG = 2,
    ROBOT_MODE_ATLER_LOG = 3,
    UART_COMMAND_LOG = 4,
    BAT_STA_UPDATA = 5,
    ERR_LOG = 6,
    ROBOT_PARAMETER_ALTER_LOG = 7,
};

typedef struct {
    uint16_t Temp;
    uint8_t batteryPercent;
    enum BAT_CHARGE_STA BatChargeSta;
    uint8_t OringBatPercenet;
    uint32_t FullTimes;
    uint32_t StaChargeTimes;
}BAT_MSG;

typedef struct {
    char LogHead[4]; //"RbLg"
    uint16_t LogSize;
    uint8_t SoftVerA;
    uint8_t SoftVerB;
    uint8_t SoftVerC;
    uint32_t LogNum;
    long long NetTimes;
    uint32_t PowerOnTimes;
    uint8_t severse[12];
    enum ROBOT_LOG_TYPE RobotLogType;
    uint16_t LogMsgSize;
    char* LogMsg;
    uint8_t CrcSum;
    char LogEnd[4];//"LgEd"


}LOG_MSG;

enum NEED_FIND_HOME_FLAG{
    FIND_HOME_WITHOUT_APPLY =  255,
    IS_NOT_NEED = 0,
    FIND_HOME_WITH_IMU,
    FIND_HOME_WITH_PHYSICAL_LIMIT,
    FIND_HOME_WITH_PHYSICAL_KEY,
};

enum ROBOT_STA{
    ROBOT_POWER_ON =  0,        //上电
    ROBOT_CALIBRATING_MOTOR,    //正在校准电机
    ROBOT_FINDING_HOME,
    ROBOT_NORMAL_STOPED,
    ROBOT_EMERGENCY_STOPED,
    ROBOT_DISABLED,
    ROBOT_CONTROL_WITH_COMMAND,
    ROBOT_WORK_WITH_PROCEDRUE,
    ROBOT_SHUTDOWN,
    ROBOT_RECOVER_FORM_DISABLE,
    ROBOT_SHUTDOWN_ING,
    ROBOT_STA_SUM,
};


typedef struct {
    float ax;
    float ay;
    float az;
    float gx;
    float gy;
    float gz;

    float axAngle;
    float ayAngle;
    float azAngle;

    float MagDir;
} FloorImuData_t;

extern FloorImuData_t FloorImuData;

typedef struct
{
    float x;
    float y;
    float z;
}Pose3D_t;

typedef struct
{
    float x;
    float y;
    float z;
    float a;
    float b;
    float c;
}Pose6D_t;

enum UP_OR_DOWN{
    UP = 0,
    DOWN,
};

typedef struct{
    uint8_t reserved0[128];

    enum HARD_VER HardVer;
    uint8_t HardVerSeriesNum;
    uint16_t HardVerRobotNum;

    uint8_t reserved1[123];

    float JointInitPose[8];

    bool JointInverseDir[8];
    float JointReduction[8];
    float JointAngleLimitMax[8];
    float JointAngleLimitMin[8];

    uint8_t reserved2[256];

    bool HavePhysicalLimit[8];
    bool PhysicalLimitInverseDir[8];
    uint16_t FindPhysicalLimitMaxCurrent[8];

    bool HaveKeyDetect[8];
    bool KeyDetectInverseDir[8];
    uint16_t FindKeyMaxCurrent[8];

    float ADJUST_ANGLE_POSITION[7];
    float ADJUST_IMU_POSITION[7];
    float ADJUST_IMU_POSITION_ERR[7];

    enum NEED_FIND_HOME_FLAG IS_NEED_FIND_HOME[8];

    float SHUTUP_POSE[7];

    uint8_t reserved3[256];

    #ifdef  BATTLE_CHARGE_THERE_AXIS_T
        #if HARD_VER_ROBOT_NUM == 1
            Pose3D_t ChargePanelPos[64];
            Pose3D_t EmptyUnitPos;
            Pose3D_t FullUnitPos;
        #else
            float workPosData[256];
        #endif
    #endif

    #ifdef  IMU6_T
        #if HARD_VER_ROBOT_NUM == 1
            Pose3D_t ChargePanelPos[64];
            Pose3D_t EmptyUnitPos;
            Pose3D_t FullUnitPos;
        #else
            float workPosData[256];
        #endif
    #endif
    #ifdef MR6_T
            Pose3D_t reservedMR6[64];
            Pose6D_t EmptyUnitPos;
            Pose6D_t FullUnitPos;
            Pose6D_t FirstBatPos;
    #endif
}USER_DATA;

//extern uint16_t ThreadControlLoopFixUpdateStep;

extern uint16_t RobotPowerOnStep,RobotFindHomeStep,RobotShutdownProcessStep,RobotNormalStopStep,RobotRecoverProcessStep;
extern bool NeedShutDownFlag;
extern enum ROBOT_STA RobotSta,RobotStaWhenStopOrDisable;
extern uint8_t PUtFullBatProcessStep,PUtEmptyBatProcessStep,TransmitProcessStep;
extern uint32_t RunToTarTimesMs,RunToTarAllTimesMs;
extern uint16_t BlockedTimesMs;
extern bool IsTarAdd[7];

extern uint8_t BatStopChargePer;

extern bool Uart1DMASendingFlag,Uart5DMASendingFlag;


extern char cmdModeNames[ROBOT_STA_SUM+1][5];

extern uint32_t RobotRunTimesMs;

extern LOG_MSG LogMsgWrite,LogMsgRead;

extern osThreadId_t DoInTimeTaskHandle;
extern osThreadId_t WriteLogTaskHandle;
extern uint32_t LogWriteNextAddr,LastLogNum;
extern bool LogWriteInitFlag;

extern bool NeedPrintLogFlag;
extern uint32_t PrintLogStartNum,PrintLogEndNum;
extern bool NeedPrintrfLogWithTimesFlag;
extern long long PrintfLogStartTimes,PrintfLogEndTimes;

extern char SprintfBuff[512];

extern bool IsUart1DmaSending();
extern bool IsUart5DmaSending();

void WriteUserDataToFalsh();

extern bool ReadUserData();
extern void InitUserDataToRobot();
extern void initUserDataToFlash();

extern unsigned int ModBusCRC16(unsigned char *data, unsigned int len);
extern bool IsModBusCRC16Right(uint8_t* p,uint16_t len);
extern bool IsCalSumRight(uint8_t* p,uint16_t len);
extern uint8_t RS485_Write(uint8_t ID_addr,uint16_t REG_addr,int32_t Tx_Data);
extern void AnaSITOCommand(uint8_t* buff,uint16_t len);
extern void ControlPanelCommand(uint8_t* buff,uint16_t len);
extern void AskSITOAngle(uint8_t id);
extern void SetSITOEnable(uint8_t id,bool En);

extern bool IfAllMotorInitPos();
extern bool IfAllMotorAdjustMag();

extern void GapCon(bool con);
void ConveyorBeltRun(bool con);
void SlopeBeltRun(bool con);
extern void BeepCon(bool con);

extern void ResponResultToComputer(uint8_t* buff,uint16_t len,uint16_t ResIdx,bool result);
extern void ResponResultToControlPanel(uint8_t* buff,uint16_t len);

extern enum HARD_VER ReadHardVer();
extern uint8_t ReadHardVerRobotNum();

extern void AskBatMsg(uint8_t PanelNum,uint8_t BatNum);
extern bool GettedChargePanelMsg;

extern void HomeKeyFun(const char *_cmd, size_t _len);
extern bool IfHomeKeyOn();
extern void HomeKeyFunPhysical();

extern void PrintSysVerMsg();

extern void PowerOnPrccess();

extern void RobotShutup();
extern void RobotRecover();
extern void RobotDisable();
extern void RobotNormalStop();
extern void RobotShutdown();
extern void RobotWorkWithProcedrue();
extern void RobotEmeraencyStop();

extern bool WriteLog(enum ROBOT_LOG_TYPE type,char* msg,uint16_t size);
extern void WriteLogToBuff(enum ROBOT_LOG_TYPE type, const char *msg);

extern bool TimestampToTimesMs(long long TimestampMs,char* timeStr);
extern bool TimesToTimestampMs(char* timeStr,long long* TimestampMs);

extern bool IsErrBat(enum BAT_CHARGE_STA code);

union UserDataUnoin{
    uint8_t UserDataBuff[USER_DATA_SOCTOR_SIZE];
    USER_DATA UserData;

};

extern union UserDataUnoin UserDataU;

extern enum NEED_FIND_HOME_FLAG NeedFindHomeFlag;
extern uint32_t TofDis;
extern uint8_t FindHomeTimes;
extern int32_t InitPosErr[7];

#ifdef __cplusplus
}
#endif


#endif //CORE_STM32F4_FW_USER_DEFINE_H

